/////////////////////////////////////////////////////////////////////////////
//
// The Neurosciences Institute, 2010
//
// File: apex_robot.h
//
// Project:
//      Conscious Artifact
//
// Author:
//       Richard G. Martin
//
// Description:
//      Provides wrapper functions for APE-X Thinklink and Walking API. 
//
/////////////////////////////////////////////////////////////////////////////

#include <iostream>
#include <nsi/thinklink/config.h>
#include <nsi/thinklink/remote_robot.h>
#include <nsi/thinklink/device.h>
#include <nsi/thinklink/error.h>
#include <stdio.h>
#include <stdlib.h>
#include <nsi/thinklink/stl.h>
#include <nsi/thinklink/scoped_ptr.h>
#include <nsi/thinklink/serial_device.h>
#include <nsi/thinklink/socket_device.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <netdb.h>
#include <string>
#include <stdexcept>
#include <unistd.h>
#include <getopt.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <netdb.h>
#include "local_options.h"

#include <nsi/walking.h>


using ::nsi::thinklink::stl::runtime_error;
using ::nsi::thinklink::serial_device;
using ::nsi::thinklink::socket_device;

using namespace std;

const	double	pi=3.1415926;
const int frame_time_ms = 30;
const int n_motors=3;
const int MAX_MOTORS = 50;
const float max_velocity_rad_s = 114.0f/60.0*2*pi; 
const float rad_per_level = 300.0f/1024.0f*pi/180.0f;


extern MPI_Comm network_comm;
extern int myid;

#ifdef DISPLAY_WINDOW
extern MPI_Comm display_comm;
extern int display_rank;
#endif

#define APEX3

#ifdef APEX1  // (yellow wiring harness.)

#define ELBOW 3
#define DELTOID 5
#define SHOULDER 6
// shoulder, deltoid, elbow
const float mins[]={200, 380, 305};
const float maxs[]={512, 657, 687};

#else // (blue wiring harness.)

#define HAND 3
#define FOREARM 6
#define ELBOW 7
#define DELTOID 9
#define SHOULDER 10
//shoulder, deltoid, elbow
//working mins, maxs used for working visuo-motor 10 pose test RGM 12/10/10
//const float mins[]= {110, 400, 310};//{100,280,310};//{185,204,404};//11/11/10->110, 450, 310
//const float maxs[]= {410, 660, 738}; //{474,630,730};//{500, 665, 738}, {492,497,643};//11/11/10->400, 650, 738

//shrink for Mental Rotation Task
const float mins[]= {5, 425, 250};//310};//{100,280,310};//{185,204,404};//11/11/10->110, 450, 310, RGM DEBUG
const float maxs[]= {410, 660, 700};//600}; //{474,630,730};//{500, 665, 738}, {492,497,643};//11/11/10->400, 650, 738

//testing mins, maxs for S1 testing, shoulder & deltoid set at maxes
//const float mins[]= {110, 500, 310};
//const float maxs[]= {450, 660, 738};
#endif

const float range_rad[] = {(maxs[0]-mins[0])*rad_per_level, (maxs[1]-mins[1])*rad_per_level,(maxs[2]-mins[2])*rad_per_level};
//const float home_pose_rad[] = { range_rad[0], range_rad[1]*0.90, range_rad[2]*0.05};
const float home_pose_rad[] = { range_rad[0]*0.9, range_rad[1]*0.9, range_rad[2]*0.05};

//mental rotation arm poses in degrees
int pose_neg_90[] = { 430, 384, 622, 575, 265, 706, 654, 355, 270, 430 };//0
int pose_neg_78[] = { 430, 384, 515, 567, 483, 645, 498, 390, 266, 430 };//1
int pose_neg_45[] = { 430, 384, 481, 561, 552, 619, 438, 357, 325, 430 };//2
int pose_neg_22[] = { 430, 384, 430, 566, 571, 552, 426, 374, 377, 430 };//3
int pose_0[] 			= { 430, 384, 380, 580, 515, 560, 444, 374, 552, 430 };//4
int pose_22[] 		= { 430, 384, 410, 580, 503, 615, 366, 344, 707, 430 };//5
int pose_45[] 		= { 430, 384, 410, 550, 503, 630, 366, 344, 765, 430 };//6

template <typename T>
T clip(T x, T minx, T maxx){
	
  return min( max(x,minx) , maxx);
}

template <typename RobotType>
struct apex_robot
  : public RobotType,
  public lv_gait_methods< apex_robot<RobotType>, RobotType >,
  public apex_methods< apex_robot<RobotType>, RobotType >,
  public apex_behave< apex_robot<RobotType>, RobotType >,
  public ir_methods,
  public misc_methods
{
  typedef ::nsi::thinklink::device device;
  
  typedef RobotType robot_type;
  typedef typename robot_type::data_type data_type;

  typedef typename data_type::actuator_type actuator_type;
  typedef typename data_type::foot_type foot_type;
  typedef typename data_type::imu_type imu_type;
  typedef typename data_type::ir_type ir_type;

  typedef typename robot_type::actuator_map_type actuator_map_type;

  // constructor
  apex_robot(){ }
  
  void init(std::string gait_file_name, ::nsi::thinklink::device* new_link)
  {
    unsigned int BLINDER_ID = 18;
    unsigned int ROTATION_ID = 19;
    unsigned int SPINNER_ID = 20;
    unsigned int spinner_object1 = 20;
    unsigned int spinner_object2 = 645;

    this->set_is_remote_robot(true);
    this->set_link(new_link);
    this->start_up( gait_file_name);
    
    /* Head Pose for Rotation Station */
    //head tilt    
    set_position(HEAD_TILT_Y, 475, 300, 700);//400, ***last on 05/09/2012 was 440 now 425 as it is in recordings project // 475 on last conditioning_v2 run
    //head pan
    set_position(HEAD_PAN_Z, 443, 300, 700);//453

    //left hand position to grab brain
    set_position(HAND_L, 570, 300, 700);//hand 570 / 510

    //left arm forearm position
    set_position(FOREARM_Z_L, 183, 300, 700);
    set_position(WRIST_X_L, 506, 300, 700);
    
    //left arm deltoid/shoulder position
    set_position(SHOULDER_Y_L, 300, 300, 700);
    set_position(SHOULDER_X_L, 640, 300, 700);
    
    //left arm wrist position
    set_position(WRIST_Y_L, 450, 300, 700);//orig pose = 478, 575
    //this->burst_transmit(&apex_robot::write);

    //tweak left arm elbow rotation
    set_position(TRICEP_Z_L, 775, 300, 700);//498, 700
    set_position(ELBOW_Y_L, 300, 300, 700); //300, 550

    //tweak right elbow
    set_position(ELBOW_Y_R, 650, 300, 700);//650
        
    //set blinder preliminary start-up position
    set_position(BLINDER_ID, 360, 300, 700);//360 => up, 30 => down

    //tweak margin for shoulder actuators for smoother motion
    this->actuator(SHOULDER_Y_L).margin_cw = 5;
    this->actuator(SHOULDER_Y_L).margin_ccw = 5;

    this->actuator(SHOULDER_X_L).margin_cw = 5;
    this->actuator(SHOULDER_X_L).margin_ccw = 5;

    //rotation station set-up
    //unsigned int rot_sta_poses[] = { 0, 306, 613, 920 }
    set_position(ROTATION_ID, 0, 306, 700);//set this value for head rotation in testing phase
    //set spinner locale
    set_position(SPINNER_ID, spinner_object1, 306, 700);//RGM
      
    //write new settings to actuators
    this->burst_transmit(&apex_robot::write);
  }

  void set_arm_pose( int min_id, int max_id, int pose_array[], int time_ms = 1500 )
  {  
    int curr_id = min_id;
    int id_range = max_id - min_id;
    int64_t secs = 0;
    int64_t usecs = 0;
		 
    this->burst_transmit(&apex_robot::query);
		
    for( int i = 0; i <= id_range; ++i, ++curr_id )
      {
	this->actuator(curr_id).position = pose_array[i];
	this->time_msec_to_time_sec_usec((int64_t)time_ms, secs, usecs); 
	this->actuator(curr_id).calculate_bounded_speed(nsi::thinklink::time(secs, usecs));
      }
    this->burst_transmit(&apex_robot::write);
  }	
  void play_arm_rotation(int pose_num, int time_step_ms = 1500)
  {
    int start_id = 11;
    int end_id = 20;
			
    switch(pose_num)
      {
      case 0:
	set_arm_pose( start_id, end_id, pose_neg_90, time_step_ms );
	break;
				
      case 1:
	set_arm_pose( start_id, end_id, pose_neg_78, time_step_ms );
	break;
				
      case 2:
	set_arm_pose( start_id, end_id, pose_neg_45, time_step_ms );
	break;
				
      case 3:
	set_arm_pose( start_id, end_id, pose_neg_22, time_step_ms );
	break;
	
      case 4:
	set_arm_pose( start_id, end_id, pose_0, time_step_ms );
	break;
					
      case 5:
	set_arm_pose( start_id, end_id, pose_22, time_step_ms );
	break;
					
      case 6:
	set_arm_pose( start_id, end_id, pose_45, time_step_ms );
	break;
						
      default:
	break;
      }
    this->burst_transmit(&apex_robot::write);
  }

  void set_position(int servo, uint16_t pos, uint16_t speed, uint16_t torque)
  {
    //std::cout << "----- set pos " << servo << ": " << this->actuator(servo).position.value() << " " << this->actuator(servo).speed.value() << " " << this->actuator(servo).torque.value() << std::endl;
    
    this->actuator(servo).position = pos;
    this->actuator(servo).speed = speed;
    this->actuator(servo).torque = torque;

    std::cout << "----- set pos " << servo << ": " << this->actuator(servo).position.value() << " " << this->actuator(servo).speed.value() << " " << this->actuator(servo).torque.value() << std::endl << std::flush;
    
    //sleep();
  }

  int get_position(int servo)
  {
    //actuator_type& act = this->actuators_[servo];
    return this->actuator(servo).present_position;
  }

  int get_speed(int servo)
  {
    //actuator_type& act = this->actuators_[servo];
    return this->actuator(servo).present_speed;
  }

  int get_torque(int servo)
  {
    //actuator_type& act = this->actuators_[servo];
    return this->actuator(servo).present_torque;
  }

  void do_state_conversions()
  { 
    const float rpm_to_rad_s = 1.0f/60.0f*2*pi;

    // positions
    for( int i = 0; i < n_motors;i++){
      state_array[i] = rad_per_level* (state_array[i]-mins[i]);
    }
    // velocities can be negative or positive.
    for( int i = n_motors; i < 2*n_motors;i++){
      state_array[i] = state_array[i] * .111 * rpm_to_rad_s;
    }
    // torques can be negative or positive.
    for( int i = 2*n_motors; i < 3*n_motors;i++){
      state_array[i] = state_array[i] * (1.0/1024.0);
    }
  }

  void get_arm_proprioception(float &shoulder, float &deltoid, float &elbow, float &sdot, float &ddot, float &edot, float &storque, float &dtorque, float &etorque, int t)
  {
    static bool first_call = true;

    if( t % frame_time_ms == 0 || first_call )
      {
	first_call = false; // just in case it's the first time.

	if( myid == 0 )
	  {
	    //DEBUG
	    //cout << " ########### Get Arm Proprioception ########## " << endl;
	    this->burst_transmit(&apex_robot::query);
        
	    state_array[0] = get_position(SHOULDER);
	    state_array[1] = get_position(DELTOID);
	    state_array[2] = get_position(ELBOW);

	    state_array[3] = get_speed(SHOULDER);
	    state_array[4] = get_speed(DELTOID);
	    state_array[5] = get_speed(ELBOW);

	    state_array[6] = get_torque(SHOULDER);
	    state_array[7] = get_torque(DELTOID);
	    state_array[8] = get_torque(ELBOW);

	    do_state_conversions();
        
	    //DEBUG
	    //cout << " >>>>>>>>>> Arm Proprioception <<<<<<<<<< " << endl << "Shoulder: " << state_array[0] << endl << "Deltoid: " << state_array[1] << endl;
	  }
	MPI_Bcast(state_array,n_motors*3, MPI_FLOAT, 0, network_comm); // Proc 0 owns the data.
      }
   
    shoulder = state_array[0];
    deltoid = state_array[1];
    elbow = state_array[2];
    sdot = state_array[3];
    ddot = state_array[4];
    edot = state_array[5];
    storque = state_array[6];
    dtorque = state_array[7];
    etorque = state_array[8];
  }

  void set_arm_command(float shoulder, float deltoid, float elbow, float sdot, float ddot, float edot, int t, int motor_update_interval_ms, int motor_update_offset_ms)
  {	
    const int max_commanded_velocity = 200;//100;
    const int torque_limit = 825;//;1000;
    const float levels_per_radian = 1.0f/rad_per_level;
    const float rad_s_to_rpm = 60.0f/(2*pi);

    if( t % motor_update_interval_ms == motor_update_offset_ms )
      {
	if( myid == 0 )
	  {
	    shoulder = clip( shoulder*levels_per_radian+mins[0],mins[0],maxs[0]);
	    deltoid  = clip( deltoid*levels_per_radian+mins[1],mins[1],maxs[1]);
	    elbow    = clip( elbow*levels_per_radian+mins[2],mins[2],maxs[2]);
        
	    set_position(SHOULDER, shoulder, clip((int)(rad_s_to_rpm * 9.0090f * sdot), 1, max_commanded_velocity), torque_limit);
	    set_position(DELTOID, deltoid, clip((int)(rad_s_to_rpm * 9.0090f * ddot), 1, max_commanded_velocity), torque_limit);
	    set_position(ELBOW, elbow, clip((int)(rad_s_to_rpm * 9.0090f * edot), 1, max_commanded_velocity), torque_limit);    
	    //set_position(FOREARM, 542, clip((int)(rad_s_to_rpm * 9.0090f * 1), 1, max_commanded_velocity), torque_limit);
	    set_position(FOREARM, 183, clip((int)(rad_s_to_rpm * 9.0090f * 1), 1, max_commanded_velocity), torque_limit); 
	    this->burst_transmit(&apex_robot::write);
	  }
      }
  }

  void write_read_arm( bool set_positions, float shoulder_goal, float deltoid_goal, float elbow_goal, 
		       float &shoulder_sens, float &deltoid_sens, float &elbow_sens, 
		       float sdot_goal, float ddot_goal, float edot_goal, 
		       float &sdot_sens, float &ddot_sens, float &edot_sens,
		       float &storque, float &dtorque, float &etorque, int t, 
		       int motor_update_interval_ms, int motor_update_offset_ms )
  {
    static bool first_call = true;
    const int max_commanded_velocity = 200;//100;
    const int torque_limit = 850;
    const float levels_per_radian = 1.0f/rad_per_level;
    const float rad_s_to_rpm = 60.0f/(2*pi);
    int64_t deltoid_time_step_ms = 1100;//500, 390
    int64_t shoulder_time_step_ms = 400;
    int64_t secs = 0;
    int64_t usecs = 0;
    int default_speed = 25;

    //cout << "write_read_arm....................................." << endl << endl;

    //cerr << " myid: " << myid << " t: " << t << " , update interval: " << motor_update_interval_ms << " offset: " << motor_update_offset_ms << " set_positions: " << set_positions << endl;
    if( ((t % motor_update_interval_ms) == motor_update_offset_ms) || first_call )
      {
	first_call = false;
	if( myid == 0 )
	  {
	    if( set_positions )
	      {
		//cerr << "set positions: write_read_arm....................................." << endl << endl;
		shoulder_goal = clip( shoulder_goal*levels_per_radian+mins[0],mins[0],maxs[0]);
		deltoid_goal  = clip( deltoid_goal*levels_per_radian+mins[1],mins[1],maxs[1]);
		elbow_goal    = clip( elbow_goal*levels_per_radian+mins[2],mins[2],maxs[2]);
        
		/*DEBUG*/
		//elbow_goal = 305;
        
		/* Set Position, Speed, & Torque */
		set_position(SHOULDER, shoulder_goal, clip((int)(rad_s_to_rpm * 9.0090f * sdot_goal), 1, max_commanded_velocity), torque_limit);
		set_position(DELTOID, deltoid_goal, clip((int)(rad_s_to_rpm * 9.0090f * ddot_goal), 1, max_commanded_velocity), torque_limit);
		set_position(ELBOW, elbow_goal, clip((int)(rad_s_to_rpm * 9.0090f * edot_goal), 1, max_commanded_velocity), torque_limit);    
		set_position(FOREARM, 183, clip((int)(rad_s_to_rpm * 9.0090f * 1), 1, max_commanded_velocity), torque_limit); 

		/* Calc Speed for Shoulder */
		this->time_msec_to_time_sec_usec((int64_t)shoulder_time_step_ms, secs, usecs); 
		this->actuator(SHOULDER).calculate_bounded_speed(nsi::thinklink::time(secs, usecs));
				  
		if( this->actuator(SHOULDER).speed < default_speed )
		  {
		    //std::cout << "Actuator " << motor_id_vec[j] << " speed = " << ptr->actuator(motor_id_vec[j]).speed << std::endl;
		    this->actuator(SHOULDER).speed = default_speed;
		  }     

		/*Calc Speed for Deltoid */
		this->time_msec_to_time_sec_usec((int64_t)deltoid_time_step_ms, secs, usecs); 
		this->actuator(DELTOID).calculate_bounded_speed(nsi::thinklink::time(secs, usecs));

		if( this->actuator(DELTOID).speed < default_speed )
		  {
		    //std::cout << "Actuator " << motor_id_vec[j] << " speed = " << ptr->actuator(motor_id_vec[j]).speed << std::endl;
		    this->actuator(DELTOID).speed = default_speed;
		  }     
	  
		/*
		// Original set-modules
		set_position(SHOULDER, shoulder_goal, clip((int)(rad_s_to_rpm * 9.0090f * sdot_goal), 1, max_commanded_velocity), torque_limit);
		set_position(DELTOID, deltoid_goal, clip((int)(rad_s_to_rpm * 9.0090f * ddot_goal), 1, max_commanded_velocity), torque_limit);
		set_position(ELBOW, elbow_goal, clip((int)(rad_s_to_rpm * 9.0090f * edot_goal), 1, max_commanded_velocity), torque_limit);    
		set_position(FOREARM, 183, clip((int)(rad_s_to_rpm * 9.0090f * 1), 1, max_commanded_velocity), torque_limit); 
		*/
	      }
				
	    /* Write Joint Commands & Automatically Read-Back Joint Sensory Data */
	    cerr << "*** Burst Transmit ***" << endl << flush;
	    this->burst_transmit(&apex_robot::write);

	    /* Set Proprioception State Arrays */
	    state_array[0] = get_position(SHOULDER);
	    state_array[1] = get_position(DELTOID);
	    state_array[2] = get_position(ELBOW);

	    state_array[3] = get_speed(SHOULDER);
	    state_array[4] = get_speed(DELTOID);
	    state_array[5] = get_speed(ELBOW);

	    state_array[6] = get_torque(SHOULDER);
	    state_array[7] = get_torque(DELTOID);
	    state_array[8] = get_torque(ELBOW);

	    do_state_conversions();

	  }//if myid
	//MPI_Bcast(state_array,n_motors*3, MPI_FLOAT, 0, network_comm); // Proc 0 owns the data.
      }//if motor_update_interval
   
    /* Set Joint Parameters */
    shoulder_sens = state_array[0];
    deltoid_sens = state_array[1];
    elbow_sens = state_array[2];
    
    sdot_sens = state_array[3];
    ddot_sens = state_array[4];
    edot_sens = state_array[5];
    
    storque = state_array[6];
    dtorque = state_array[7];
    etorque = state_array[8];

    //cout << "exiting write_read_arm....................................." << endl << endl;
  }
	
  
  void update_arm_joints( bool set_positions, float shoulder_goal, float deltoid_goal, float elbow_goal,
                                              int shoulder_speed_time_ms, int deltoid_speed_time_ms, int elbow_speed_time_ms,
                                              float storque, float dtorque, float etorque, 
                                              float &shoulder_sens, float &deltoid_sens, float &elbow_sens, 
                                              float &sdot_sens, float &ddot_sens, float &edot_sens,
                                              float &storque_sens, float &dtorque_sens, float &etorque_sens,
                                              int t, int motor_update_interval_ms, int motor_update_offset_ms )
  {
    static bool first_call = true;
    const int max_commanded_velocity = 200;//100;
    const int torque_limit = 850;
    const float levels_per_radian = 1.0f/rad_per_level;
    const float rad_s_to_rpm = 60.0f/(2*pi);
    
    int64_t shoulder_time_step_ms = shoulder_speed_time_ms;//400;
    int64_t deltoid_time_step_ms = deltoid_speed_time_ms;//1100;//500, 390
    int64_t elbow_time_step_ms = elbow_speed_time_ms;
    int64_t secs = 0;
    int64_t usecs = 0;
    int default_speed = 25;

    //cout << "write_read_arm....................................." << endl << endl;

    //cerr << " myid: " << myid << " t: " << t << " , update interval: " << motor_update_interval_ms << " offset: " << motor_update_offset_ms << " set_positions: " << set_positions << endl;
    if( ((t % motor_update_interval_ms) == motor_update_offset_ms) || first_call )
    {
      first_call = false;
      if( myid == 0 )
      {
        if( set_positions )
	      {
          //cerr << "set positions: write_read_arm....................................." << endl << endl;
          shoulder_goal = clip( shoulder_goal*levels_per_radian+mins[0],mins[0],maxs[0]);
          deltoid_goal  = clip( deltoid_goal*levels_per_radian+mins[1],mins[1],maxs[1]);
          elbow_goal    = clip( elbow_goal*levels_per_radian+mins[2],mins[2],maxs[2]);
              
              
          /* Set Position, Speed, & Torque */
          set_position(SHOULDER, shoulder_goal, 100, storque);
          set_position(DELTOID, deltoid_goal, 100, dtorque);
          set_position(ELBOW, elbow_goal, 100, etorque);    
          
          /* Calc Speed for Shoulder */
          this->time_msec_to_time_sec_usec((int64_t)shoulder_time_step_ms, secs, usecs); 
          this->actuator(SHOULDER).calculate_bounded_speed(nsi::thinklink::time(secs, usecs));
                
          if( this->actuator(SHOULDER).speed < default_speed )
          {
            //std::cout << "Actuator " << motor_id_vec[j] << " speed = " << ptr->actuator(motor_id_vec[j]).speed << std::endl;
            this->actuator(SHOULDER).speed = default_speed;
          }     

          /*Calc Speed for Deltoid */
          this->time_msec_to_time_sec_usec((int64_t)deltoid_time_step_ms, secs, usecs); 
          this->actuator(DELTOID).calculate_bounded_speed(nsi::thinklink::time(secs, usecs));

          if( this->actuator(DELTOID).speed < default_speed )
          {
            //std::cout << "Actuator " << motor_id_vec[j] << " speed = " << ptr->actuator(motor_id_vec[j]).speed << std::endl;
            this->actuator(DELTOID).speed = default_speed;
          }     
          
          /*Calc Speed for Elbow */
          this->time_msec_to_time_sec_usec((int64_t)elbow_time_step_ms, secs, usecs); 
          this->actuator(ELBOW).calculate_bounded_speed(nsi::thinklink::time(secs, usecs));

          if( this->actuator(ELBOW).speed < default_speed )
          {
            //std::cout << "Actuator " << motor_id_vec[j] << " speed = " << ptr->actuator(motor_id_vec[j]).speed << std::endl;
            this->actuator(ELBOW).speed = default_speed;
          } 
	      }
				
        /* Write Joint Commands & Automatically Read-Back Joint Sensory Data */
        cerr << "*** Burst Transmit ***" << endl << flush;
        this->burst_transmit(&apex_robot::write);

        /* Set Proprioception State Arrays */
        state_array[0] = get_position(SHOULDER);
        state_array[1] = get_position(DELTOID);
        state_array[2] = get_position(ELBOW);

        state_array[3] = get_speed(SHOULDER);
        state_array[4] = get_speed(DELTOID);
        state_array[5] = get_speed(ELBOW);

        state_array[6] = get_torque(SHOULDER);
        state_array[7] = get_torque(DELTOID);
        state_array[8] = get_torque(ELBOW);

        do_state_conversions();

      }//if myid
      //MPI_Bcast(state_array,n_motors*3, MPI_FLOAT, 0, network_comm); // Proc 0 owns the data.
    }//if motor_update_interval
   
    /* Set Joint Parameters */
    shoulder_sens = state_array[0];
    deltoid_sens = state_array[1];
    elbow_sens = state_array[2];
    
    sdot_sens = state_array[3];
    ddot_sens = state_array[4];
    edot_sens = state_array[5];
    
    storque_sens = state_array[6];
    dtorque_sens = state_array[7];
    etorque_sens = state_array[8];

    //cout << "exiting write_read_arm....................................." << endl << endl;
  }
	
#ifdef DISPLAY_WINDOW
  // transmit motor data to display processor
  void transmit_to_display (void)
  {
    int s, Ndata;
    int *data;
#ifdef LA
    Ndata = 8 * 3;
    data = (int *)malloc(Ndata * sizeof(int));
    for (s = 0; s < 8; s++)
      {
	data[3 * s + 0] = get_position (s+3);
	data[3 * s + 1] = get_speed (s+3);
	data[3 * s + 2] = get_torque (s+3);
      }
    MPI_Send (&Ndata, 1, MPI_INT, 0, 700, display_comm);
    MPI_Send (data, Ndata, MPI_INT, 0, 701, display_comm);
    free (data);
#endif
#ifdef RA
    Ndata = 8 * 3;
    data = (int *)malloc(Ndata * sizeof(int));
    for (s = 0; s < 8; s++)
      {
	data[3 * s + 0] = get_position (s+13);
	data[3 * s + 1] = get_speed (s+13);
	data[3 * s + 2] = get_torque (s+13);
      }
    MPI_Send (&Ndata, 1, MPI_INT, 0, 700, display_comm);
    MPI_Send (data, Ndata, MPI_INT, 0, 701, display_comm);
    free (data);
#endif
#ifdef LL
    Ndata = 6 * 3;
    data = (int *)malloc(Ndata * sizeof(int));
    for (s = 0; s < 6; s++)
      {
	data[3 * s + 0] = get_position (s+23);
	data[3 * s + 1] = get_speed (s+23);
	data[3 * s + 2] = get_torque (s+23);
      }
    MPI_Send (&Ndata, 1, MPI_INT, 0, 700, display_comm);
    MPI_Send (data, Ndata, MPI_INT, 0, 701, display_comm);
    free (data);
#endif
#ifdef RL		
    Ndata = 6 * 3;
    data = (int *)malloc(Ndata * sizeof(int));
    for (s = 0; s < 6; s++)
      {
	data[3 * s + 0] = get_position (s+30);
	data[3 * s + 1] = get_speed (s+30);
	data[3 * s + 2] = get_torque (s+30);
      }
    MPI_Send (&Ndata, 1, MPI_INT, 0, 700, display_comm);
    MPI_Send (data, Ndata, MPI_INT, 0, 701, display_comm);
    free (data);
#endif
#ifdef B
    Ndata = 3 * 3;
    data = (int *)malloc(3 * 3 * sizeof(int));
    data[0] = get_position (11);
    data[1] = get_speed (11);
    data[2] = get_torque (11);
    data[3] = get_position (12);
    data[4] = get_speed (12);
    data[5] = get_torque (12);
    data[6] = get_position (29);
    data[7] = get_speed (29);
    data[8] = get_torque (29);
    MPI_Send (&Ndata, 1, MPI_INT, 0, 700, display_comm);
    MPI_Send (data, Ndata, MPI_INT, 0, 701, display_comm);
    free (data);
#endif
  }
#endif

 protected:

  int time_;
  bool single_;
  float state_array[3*MAX_MOTORS];  

};


    
